#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTServo,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     Ultra,          sensorSONAR)
#pragma config(Sensor, S4,     HTSMUX,         sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     B_Left,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     F_Left,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     Lift,          tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     Scooper,       tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     Launcher,      tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C2_2,     Raise,         tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C3_1,     B_Right,       tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C3_2,     F_Right,       tmotorTetrix, PIDControl, encoder)
#pragma config(Servo,  srvo_S2_C1_1,    Hook,                 tServoStandard)
#pragma config(Servo,  srvo_S2_C1_2,    Guide,                tServoStandard)
#pragma config(Servo,  srvo_S2_C1_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "AutonomousBase.c"
#include "JoystickDriver.c"

//#pragma DebuggerWindows("debugStream")

//drive_full(char dir, int dist, int speed, int lExt, int scoopT, float dScoop, int raised, int maintain, int maint_range)

void initializeRobot()
{
	motor[B_Right] = 0;					//Reset all motor values.
	motor[B_Left] = 0;
	motor[F_Right] = 0;
	motor[F_Left] = 0;
	motor[Launcher] = 0;
	motor[Raise] = 0;
	motor[Lift] = 0;
	motor[Scooper] = 0;
	resetEncoders();
	servoTarget[Hook] = 50;			//Sets hook up
	servoTarget[Guide] = 237;
}
int things[7] = {B_Left, B_Right, F_Left, F_Right, Launcher, Raise, Lift};
task main()
{


	int raiseST;
	while(true)
	{
		raiseST = TSreadState(BtnRA);
		writeDebugStreamLine("raiseST = %d", raiseST);
		wait10Msec(50);
	}

}
